#include "mjbots.h"
#include "fdcan_interface.h"

typedef struct
{
    float mode;
    float position;
    float velocity;
    float torque;
    uint8_t recv_flag;
}motor_status_s;

motor_status_s motor_status[4];

/*********
* @brief 电机电压控制，单位是0.01v
*********/
void set_dq_volt(motor_e motor, int16_t volt)
{
    uint8_t cmd[] = {0x01, 0x00, 0x08, 0x06, 0x1a, 0x00, 0x00, 0x00, 0x00, 0x18, 0x04, 0x00};
    *(int16_t*)&cmd[7] = volt;
    push_queue(0x8000 | motor, cmd, sizeof(cmd));
}

/*********
* @brief 电机力矩/电流控制，单位是0.01A
*********/
void set_torque(motor_e motor, int16_t torque)
{
    uint8_t cmd[] = {0x01, 0x00, 0x09, 0x06, 0x1C, 0x06, 0x00, 0x00, 0x00, 0x18, 0x04, 0x00};
    *(int16_t*)&cmd[5] = torque;
    push_queue(0x8000 | motor, cmd, sizeof(cmd));
}

/*********
* @brief 电机位置-速度-前馈力矩控制，单位是0.0001圈
*********/
void set_position(motor_e motor, int16_t pos, int16_t vel, int16_t torque)
{
    uint8_t cmd[] = {0x01, 0x00, 0x0A, 0x07, 0x20, 0x00, 0x00, 0X20, 0x02, 0x50, 0xFF, 0x18, 0x04, 0x00, 0X13, 0X0D};
    *(int16_t*)&cmd[5] = pos;
    *(int16_t*)&cmd[7] = vel;
    *(int16_t*)&cmd[9] = torque;
    push_queue(0x8000 | motor, cmd, sizeof(cmd));
}

/*********
* @brief 电机速度控制，单位是0.0001圈
*********/
void set_velocity(motor_e motor, int16_t vel, int16_t torque)
{
    uint8_t cmd[] = {0x01, 0x00, 0x0A, 0x07, 0x20, 0x00, 0x00, 0X20, 0x02, 0x50, 0xFF, 0x18, 0x04, 0x00, 0X13, 0X0D};
    *(uint16_t*)&cmd[5] = 0x8000;
    *(int16_t*)&cmd[7] = vel;
    *(int16_t*)&cmd[9] = torque;
    push_queue(0x8000 | motor, cmd, sizeof(cmd));
}

/*********
* @brief 电机位置-速度-前馈力矩控制，单位是0.00001圈
*********/
void set_position_32(motor_e motor, int32_t pos, int32_t vel, int32_t torque)
{
    __IO uint8_t cmd[] = {0x01, 0x00, 0x0A, 0x0B, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x04, 0x00};
    *(int32_t*)&cmd[5] = pos;
    *(int32_t*)&cmd[9] = vel;
    *(int32_t*)&cmd[13] = torque;
    push_queue(0x8000 | motor, (uint8_t*)cmd, sizeof(cmd));
}

/*********
* @brief 电机位置-速度-前馈力矩控制，单位是0.00001圈
*********/
void set_position_32_kpkd(motor_e motor, int32_t pos, int32_t vel, int32_t torque, float kp, float kd)
{
    __IO uint8_t cmd[] = {0x01, 0x00, 0x0A, 0x0B, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
                          0x00, 0x06, 0x23, 0x3F, 0xFF, 0x3F, 0xFF, 0x18, 0x04, 0x00, 0x50, 0x50, 0x50, 0x50, 0x50, 0x50};
    *(int32_t*)&cmd[5] = pos;
    *(int32_t*)&cmd[9] = vel;
    *(int32_t*)&cmd[13] = torque;
    *(int16_t*)&cmd[19] = (int16_t)(kp * 32767);
    *(int16_t*)&cmd[21] = (int16_t)(kd * 32767);
    push_queue(0x8000 | motor, (uint8_t*)cmd, sizeof(cmd));
}

/*********
* @brief 电机速度-前馈力矩控制，单位是0.00001圈
*********/
void set_velocity_32(motor_e motor, int32_t vel, int32_t torque)
{
    uint8_t cmd[] = {0x01, 0x00, 0x0A, 0x0B, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x04, 0x00};
    *(uint32_t*)&cmd[5] = 0x80000000;
    *(int32_t*)&cmd[9] = vel;
    *(int32_t*)&cmd[13] = torque;
    push_queue(0x8000 | motor, cmd, sizeof(cmd));
}

void set_max_velocity(motor_e motor, int16_t max_vel)
{
    uint8_t cmd[] = {0x05, 0x28, 0x00, 0x00, 0x18, 0x04, 0x00};
    *(int16_t*)&cmd[2] = max_vel;
    push_queue(0x8000 | motor, cmd, sizeof(cmd));
}

void set_max_acc(motor_e motor, int16_t max_acc)
{
    uint8_t cmd[] = {0x05, 0x29, 0x00, 0x00, 0x18, 0x04, 0x00};
    *(int16_t*)&cmd[2] = max_acc;
    push_queue(0x8000 | motor, cmd, sizeof(cmd));
}

void request_motor_status(motor_e motor)
{
    uint8_t cmd[] = {0x18, 0x04, 0x00};
    push_queue(0x8000 | motor, cmd, sizeof(cmd));
}


void parse_mjbots(uint8_t* RxData, uint16_t id)
{
    id = id >> 8;
    if(id >= 1 && id <= 4)
    {
        if(RxData[0] == 0x28 && RxData[1] == 0x04 && RxData[2] == 0x00)
        {
            motor_status[id - 1].mode = *(int32_t*)&RxData[3];
            motor_status[id - 1].position = *(int32_t*)&RxData[7] * 0.00001;  // 圈数
            motor_status[id - 1].velocity = *(int32_t*)&RxData[11] * 0.00001; // 转速n/s
            motor_status[id - 1].torque = *(int32_t*)&RxData[13] * 0.001;      // 转矩 N-M     
            motor_status[id - 1].recv_flag = 1;
        }
    }
}

uint8_t is_get_motor_status(motor_e motor)
{
    return motor_status[motor-1].recv_flag;
}

float get_motor_mode(motor_e motor)
{
    motor_status[motor-1].recv_flag = 0;
    return motor_status[motor - 1].mode;
}

float get_motor_position(motor_e motor)
{
    motor_status[motor-1].recv_flag = 0;
    return motor_status[motor - 1].position;
}

float get_motor_velocity(motor_e motor)
{
    motor_status[motor-1].recv_flag = 0;
    return motor_status[motor - 1].velocity;
}

float get_motor_torque(motor_e motor)
{
    motor_status[motor-1].recv_flag = 0;
    return motor_status[motor - 1].torque;
}

